#include "application.h"
#include "drv_l1_usbh_ohci.h"
#include "drv_l1_usbh_ehci.h"
#include "drv_l2_usbh.h"
#include "drv_l2_usbh_uvc.h"

#if ENABLE_REAR_SENSOR_FROM_HOST_UVC
/**************************************************************************
* 					 C O N S T A N T S									  *
**************************************************************************/
#define USBH_UVC_TASK_QUEUE_MAX			16
#define USBH_UVC_TASK_QUEUE_WAIT_CNT	5	/* 50ms */

INT32U fps_uvc_frame;
INT32U fps_uvc_update;
INT32U fps_uvc_size;
INT32U fps_uvc_max_size;
INT32U fps_uvc_overflow;

void *usbh_uvc_task_q_stack[USBH_UVC_TASK_QUEUE_MAX];

enum
{
	UVC_NONE_CMD = 0,
	UVC_START_GET_FRAME_CMD,
	UVC_STOP_GET_FRAME_CMD
};

/*enum
{
	UVC_DEV_STATE_DEVICE_NOT_FOUND = 0,
	UVC_DEV_STATE_DEVICE_PLUG_IN,
	UVC_DEV_STATE_DEVICE_READY,
	UVC_DEV_STATE_GETTING_JPEG,
	UVC_DEV_STATE_SEND_GET_FRAME_TIMEOUT,
	UVC_DEV_STATE_SEND_SETUP_CMD_FAILED
};*/

/* For USB host reinit flag */
enum
{
	UVC_NO_REINIT = 0,
	UVC_USB_DEVICE_EXIST_REINIT,
	UVC_HOST_COMMAND_FAILED_REINIT,
	UVC_HOST_TIMEOUT_REINIT,
	UVC_HOST_AB_BUF_OVERFLOW_REINIT
};

static char *uvc_state_string[] =
{
	"Not Found",
	"Plug IN",
	"Ready",
	"Getting JPEG",
	"Send Get Frame Timeout",
	"Send Setup Command Failed"
};

INT32U	uvc_device_state = UVC_DEV_STATE_DEVICE_NOT_FOUND;
static INT32U	uvc_host_init_flag = 0;
INT32U	uvc_host_reinit_flag = UVC_NO_REINIT;
static INT32U	uvc_host_current_cmd = UVC_NONE_CMD;
static char		pre_parking_status = 0;

#define UVC_HOST_INVALID_FRAME_CNT	2
#define UVC_HOST_VBUS_RESET_CNT		20	/* 100ms */
#define UVC_HOST_REINIT_CNT			20	/* 200ms */
static INT32U	uvc_frame_drop_cnt;
INT8U gWait_Stop_Get_ISO_Data = 0;
INT8U gStop_Get_ISO_Data = 0;
extern INT8U gUSB20_11_Connect;
extern INT8U gDetect_USBHUVC_Plug;
INT8U g_usb_host_UVC_data_error_count = 0;
INT8U gContinue_Record = 0;
extern INT8U iTD_group_idx;
INT8U gJpeg_clk_close = 0;

#define USBH_TXDE_VAL	1	/* 0~7 */
#define USBH_RXEQ_VAL	0	/* 0~3 */

/**************************************************************************
* 					 G L O B A L		D A T A   						  *
**************************************************************************/
OS_EVENT		*USBHUVCTaskQ;
static OS_EVENT 	*uvc_sem = NULL;
static OS_EVENT 	*usb_phy_sem = NULL;
static INT32U	uvc_jpeg_frame_buf_size;

void uvc_host_init(void);
void uvc_host_uninit(void);
extern INT32U uvc_q_get_num(void);
extern INT32U uvc_q_get_size(void);
extern INT32U uvc_q_get_pad_size(void);
extern void mm_dump(void);

INT8U change_resolution_flag = 0;

extern INT32U	rear_jpeg_in_addr;
extern INT32U uvc_keep_buf;

extern INT8U USB_first_jpg;

/**************************************************************************/
static void uvc_setup_cmd_lock(void)
{
	INT8U err;
	if(uvc_sem)
	{
		OSSemPend(uvc_sem, 0, &err);
	}
}

static void uvc_setup_cmd_unlock(void)
{
	if(uvc_sem)
	{
		OSSemPost(uvc_sem);
	}
}

void usb_phy_lock(void)
{
	INT8U err;
	if(usb_phy_sem)
	{
		OSSemPend(usb_phy_sem, 0, &err);
	}
}

void usb_phy_unlock(void)
{
	if(usb_phy_sem)
	{
		OSSemPost(usb_phy_sem);
	}
}

void uvc_host_vbus_control(INT32U enable)
{
	gpio_write_io(BACK_SENSOR_PWR, enable?1:0);
}

static void uvc_event_cbk(INT32U event, INT32U done_buf, INT32U size)
{
	UVC_FRAME_BUF_INFO	frame_buf_info;
	char* reverse_parking_sta;
	char cur_parking_status = 0;
	INT8U* test_ptr;
	INT32U j = 0;
	switch(event)
	{
	case UVC_FRAME_A_DONE_EVENT:
	case UVC_FRAME_A_OVERFLOW_EVENT:
		/* Update frame A buffer */
		if(uvc_frame_drop_cnt)
		{
			--uvc_frame_drop_cnt;
			frame_buf_info.abuf_addr = DUMMY_BUFFER_ADDRS;
		}
		else
		{
			if(uvc_q_get_idle(&(frame_buf_info.abuf_addr)) == STATUS_FAIL)
			{
				frame_buf_info.abuf_addr = DUMMY_BUFFER_ADDRS;
			}
		}

		frame_buf_info.abuf_size = uvc_jpeg_frame_buf_size;
		frame_buf_info.bbuf_addr = NULL;
		frame_buf_info.bbuf_size = 0;
		drv_l2_usbh_uvc_update_frame_buf(&frame_buf_info);

		if(event == UVC_FRAME_A_OVERFLOW_EVENT)
		{
			DBG_PRINT("UVC A buf overflow 0x%X\r\n", done_buf);
			if(done_buf != DUMMY_BUFFER_ADDRS)
			{
				uvc_q_put_idle(done_buf);
			}
			break;
		}

		// Send to decode
		if(done_buf != DUMMY_BUFFER_ADDRS)
		{
			++fps_uvc_update;
			cache_invalid_range(done_buf, size);

			test_ptr = (INT8U*) done_buf;
			 for(j=(size-1);j>(size-512);j--)
				{
					if((test_ptr[j]==0xD9)&&(test_ptr[j-1]==0xFF))
					{
						size = j+1;
						break;
					}
				}
			if(test_ptr[0] != 0xFF || test_ptr[1] != 0xD8 || test_ptr[size-2] != 0xFF || test_ptr[size-1] != 0xD9)
			{
				DBG_PRINT("zzy [ERROR] A buf 0x%X= 0x%X 0x%X ... 0x%X 0x%X\r\n", done_buf, test_ptr[0], test_ptr[1], test_ptr[size-2], test_ptr[size-1]);
				uvc_q_put_idle(done_buf);
				break;
			}

			uvc_q_put_ready(done_buf, size);
		}
		//DBG_PRINT("A buf done 0x%x size =0x%x\r\n", done_buf, size);
		break;

	case UVC_FRAME_B_DONE_EVENT:
	case UVC_FRAME_B_OVERFLOW_EVENT:
		/* Update frame b buffer */
		if(uvc_frame_drop_cnt)
		{
			--uvc_frame_drop_cnt;
			frame_buf_info.bbuf_addr = DUMMY_BUFFER_ADDRS;
		}
		else
		{
			if(uvc_q_get_idle(&(frame_buf_info.bbuf_addr)) == STATUS_FAIL)
			{
				frame_buf_info.bbuf_addr = DUMMY_BUFFER_ADDRS;
			}
		}

		frame_buf_info.abuf_addr = NULL;
		frame_buf_info.abuf_size = 0;
		frame_buf_info.bbuf_size = uvc_jpeg_frame_buf_size;
		drv_l2_usbh_uvc_update_frame_buf(&frame_buf_info);

		if(event == UVC_FRAME_B_OVERFLOW_EVENT)
		{
			DBG_PRINT("UVC B buf overflow 0x%X\r\n", done_buf);
			if(done_buf != DUMMY_BUFFER_ADDRS)
			{
				uvc_q_put_idle(done_buf);
			}
			break;
		}
	
		// Send to decode 
		if(done_buf != DUMMY_BUFFER_ADDRS)
		{
			++fps_uvc_update;
			cache_invalid_range(done_buf, size);

			test_ptr = (INT8U*) done_buf;
			 for(j=(size-1);j>(size-512);j--)
				{
					if((test_ptr[j]==0xD9)&&(test_ptr[j-1]==0xFF))
					{
						size = j+1;
						break;
					}
				}
			if(test_ptr[0] != 0xFF || test_ptr[1] != 0xD8 || test_ptr[size-2] != 0xFF || test_ptr[size-1] != 0xD9)
			{
				DBG_PRINT("zzy [ERROR] B buf 0x%X= 0x%X 0x%X ... 0x%X 0x%X\r\n", done_buf, test_ptr[0], test_ptr[1], test_ptr[size-2], test_ptr[size-1]);
				uvc_q_put_idle(done_buf);
				break;
			}

			uvc_q_put_ready(done_buf, size);
		}
		//DBG_PRINT("B buf done 0x%x size =0x%x\r\n", done_buf, size);
		break;

	case UVC_PORT_DISCONNECTED_EVENT:
		DBG_PRINT("Host: USB host port disconnected\r\n");
		break;

	case UVC_PORT_CONNECTED_EVENT:
		DBG_PRINT("Host: USB host port connected\r\n");
		break;

	case UVC_REVERSE_PARKING_STATUS_EVENT:
		reverse_parking_sta = (char*)done_buf;
		cur_parking_status = (*(reverse_parking_sta+2) & 0x1);
		//DBG_PRINT("RP status [0x%x] pre_parking_status %d cur_parking_status %d\r\n", *(reverse_parking_sta+2), pre_parking_status, cur_parking_status);
		if(pre_parking_status != cur_parking_status)
		{
			pre_parking_status = cur_parking_status;
			if(pre_parking_status)
			{
				msgQSend(PeripheralTaskQ, MSG_PERIPHERAL_BACK_SENSOR_REVERSE_PRAKING_DETECTED, NULL, NULL, MSG_PRI_NORMAL);
			}
			else
			{
				msgQSend(PeripheralTaskQ, MSG_PERIPHERAL_BACK_SENSOR_REVERSE_PRAKING_REMOVED, NULL, NULL, MSG_PRI_NORMAL);
			}
		}
		break;

	case UVC_FRAME_SEND_ITD_TIMEOUT_EVENT:
		uvc_device_state = UVC_DEV_STATE_SEND_GET_FRAME_TIMEOUT;
		break;

	case UVC_TASK_TIMEOUT_EVENT:
		///DBG_PRINT("UVC_TASK_TIMEOUT_EVENT[%s] device io %d\r\n", uvc_state_string[uvc_device_state], ap_peripheral_get_usbd_io_status());
		if(uvc_device_state == UVC_DEV_STATE_GETTING_JPEG)
		{
			//DBG_PRINT("Sending JPEG but UVC_TASK_TIMEOUT_EVENT io %d\r\n", ap_peripheral_get_usbd_io_status());
			if(!ap_peripheral_get_usbd_io_status())
			{
				if(drv_l1_usbh_detect_port())
				{
					/* Check if USB device io is exist */
					uvc_host_reinit_flag = UVC_HOST_TIMEOUT_REINIT;
					uvc_host_current_cmd = UVC_START_GET_FRAME_CMD;
					DBG_PRINT("Host: UVC timeout send plug out\r\n");
					OSQPost(USBHUVCTaskQ, (void *) MSG_USB_HOST_UVC_PLUG_OUT);
				}
			}
		}
		break;

	case UVC_SEND_SETUP_CMD_FAILED_EVENT:
		DBG_PRINT("Host: UVC_SEND_SETUP_CMD_FAILED_EVENT[%s]\r\n", uvc_state_string[uvc_device_state]);
		uvc_setup_cmd_unlock();
		uvc_host_reinit_flag = UVC_HOST_COMMAND_FAILED_REINIT;
		uvc_host_current_cmd = UVC_START_GET_FRAME_CMD;
		uvc_device_state = UVC_DEV_STATE_SEND_SETUP_CMD_FAILED;
		DBG_PRINT("Setup cmd failed send plug out\r\n");
		
		#if defined(BACK_SENSOR_NON_PIN_DET) && BACK_SENSOR_NON_PIN_DET
			uvc_host_reinit_flag = UVC_NO_REINIT;
			uvc_host_current_cmd = UVC_NONE_CMD;				
			msgQSend(ApQ, MSG_APQ_BACK_SENSOR_PLUG_OUT, NULL, 0, MSG_PRI_NORMAL);
		#endif
		
		OSQPost(USBHUVCTaskQ, (void *) MSG_USB_HOST_UVC_PLUG_OUT);
		break;

	case UVC_SEND_SETUP_CMD_SUCCESSFUL_EVENT:
		uvc_setup_cmd_unlock();
		break;

	default:
		break;
	}
}

/* Call in EHCI layer interrupt environment */
static void ehci_uvc_event_cbk(INT32U event)
{
	INT32U msg;
	///DBG_PRINT("ehci_uvc_event_cbk event %d\r\n", event);
	switch(event)
	{
		case EHCI_HOST_UPDATE_FRAME_LIST_EVENT:
			msg = MSG_USB_HOST_UVC_UPDATE_FRAME_LIST_EVENT;
			break;
		
		case EHCI_HOST_ISO_FRAME_A_DONE_EVENT:
			msg = MSG_USB_HOST_UVC_ABUF_DONE;
			break;
			
		case EHCI_HOST_ISO_FRAME_A_OVERFLOW_EVENT:
			msg = MSG_USB_HOST_UVC_A_OVERFLOW_EVENT;
			break;
		
		case EHCI_HOST_ISO_FRAME_B_DONE_EVENT:
			msg = MSG_USB_HOST_UVC_BBUF_DONE;
			break;
			
		case EHCI_HOST_ISO_FRAME_B_OVERFLOW_EVENT:
			msg = MSG_USB_HOST_UVC_B_OVERFLOW_EVENT;
			break;
		
		case EHCI_HOST_PORT_PLUG_OUT_EVENT:
			msg = MSG_USB_HOST_UVC_PLUG_OUT;
			uvc_host_reinit_flag = UVC_USB_DEVICE_EXIST_REINIT;
			break;	
	}
	
	OSQPost(USBHUVCTaskQ ,(void*)msg);
}

void uvc_init_jpeg_buffer(void)
{
	UVC_FRAME_BUF_INFO	frame_buf_info;

	/* Assign A/B buffer info */
	frame_buf_info.abuf_addr = DUMMY_BUFFER_ADDRS;
	frame_buf_info.bbuf_addr = DUMMY_BUFFER_ADDRS;

	uvc_jpeg_frame_buf_size = uvc_q_get_size();

	frame_buf_info.abuf_size = uvc_jpeg_frame_buf_size;
	frame_buf_info.bbuf_size = uvc_jpeg_frame_buf_size;

	uvc_frame_drop_cnt = UVC_HOST_INVALID_FRAME_CNT;

       if(gUSB20_11_Connect == 1)     // USB2.0
           drv_l2_usbh_ehci_uvc_register_frame_info(&frame_buf_info);
       else    // USB1.1
	    drv_l2_usbh_uvc_register_frame_info(&frame_buf_info);
}

void uvc_free_jpeg_buffer(void)
{
	uvc_q_reinit();
}

void uvc_host_init(void)
{
	if(uvc_host_init_flag == 0)
	{
		if(gUSB20_11_Connect == 1)     // USB2.0
		{
			drv_l2_usbh_ehci_uvc_init();

			drv_l1_usbh_ehci_iso_register_frame_handler(ehci_uvc_event_cbk);

			drv_l1_usbh_register_host_operation_table(&ehci_op_tbl);

			drv_l1_usbh_phy_clk_on();
			OSTimeDly(1);

			drv_l1_usbh_init();

			drv_l1_usbh_ehci_iso_enable_buf_size(1, uvc_q_get_size());

			/* Config PHY for USB20 */
			drv_l1_usbd_phy_config(USBD_PHY_SETTING);
		}
		else     // USB1.1
		{
		    /* Init USB host UVC layer 2 driver */
		    drv_l2_usbh_uvc_init(PRIORITY_USBH_L2);

		    /* register frame buffer done call back */
		    drv_l2_usbh_uvc_register_frame_done_cbk(uvc_event_cbk);

		   /* Regsiter OHCI layer 1 driver first */
		   drv_l1_usbh_register_host_operation_table(&ohci_op_tbl);

		   /* Enable host PHY clock */
		   drv_l1_usbh_phy_clk_on();

		   /* Init USB host layer 1 driver */
		   drv_l1_usbh_init();

		   /* Limit A/B buffer size to avoid buffer overflow */
		   drv_l1_usbh_ohci_iso_enable_buf_size(1, uvc_q_get_size());
		}

		uvc_host_init_flag = 1;
		pre_parking_status = 0;
		g_usb_host_UVC_data_error_count = 0;    // clear data error count
		
		#if defined(C_UVC_MSG_DEBUG) && C_UVC_MSG_DEBUG
		DBG_PRINT("Host: uvc_host_init\r\n");
		#endif
		
	}
}

void uvc_host_uninit(void)
{
	if(uvc_host_init_flag == 1)
	{
              if(gUSB20_11_Connect == 1)    //USB2.0
              {
                  drv_l1_usbh_ehci_iso_enable_buf_size(0, uvc_q_get_size());

		    drv_l1_usbh_uninit();

		    drv_l1_usbh_phy_clk_off();

		    /* Flush Q and wait for plug in or out event */
		    OSQFlush(USBHUVCTaskQ);
              }
	       else     // USB1.1
	       {
	           drv_l2_usbh_uvc_uninit();

		    /* register frame buffer done call back */
		    drv_l2_usbh_uvc_register_frame_done_cbk(NULL);

		    drv_l2_usbh_release();
	       }
		
		uvc_host_init_flag = 0;
		g_usb_host_UVC_data_error_count = 0;    // clear data error count

	#if defined(C_UVC_MSG_DEBUG) && C_UVC_MSG_DEBUG
		DBG_PRINT("Host: uvc_host_uninit\r\n");
	#endif
		
	}
}

#if defined(BACK_SENSOR_NON_PIN_DET) && BACK_SENSOR_NON_PIN_DET
#define	UVC_HOST_NONPIN_RETRY_CNT		5
INT32U UVC_HOST_NONPIN_INI_CNT=0;
INT8U UVC_HOS_NONPINT_INIT_FLAG=0;
extern INT8U in_USBD_mode_flag;
extern INT8U in_UVC_mode_flag;
#endif
extern INT8U gApp_Select_Res;
extern INT8U gUSB_CAM_CONNECT;
extern INT16U present_state;
extern void ap_video_record_clear_resolution_str(void);
extern void ap_capture_resolution_clear(void);

void switch_res_setting(void)
{
	/*if(gApp_Select_Res==1)//720p
	{
		pAviEncVidPara->sensor_width_r	= 1280;
		pAviEncVidPara->sensor_height_r	= 720;
		pAviEncVidPara->encode_width_r	= 1280;
		pAviEncVidPara->encode_height_r	= 720;
	}
	else
	{
		pAviEncVidPara->sensor_width_r	= 640;
		pAviEncVidPara->sensor_height_r	= 480;
		pAviEncVidPara->encode_width_r	= 640;
		pAviEncVidPara->encode_height_r	= 480;
	}*/
	///if(change_resolution_flag==1) return;
	if((present_state==STATE_VIDEO_RECORD)||(present_state==STATE_PHOTO_CAPTURE))
	{
		ap_video_record_clear_resolution_str();
		ap_capture_resolution_clear();
		video_encode_sensor_off();
		OSTimeDly(2);
		
		if(present_state==STATE_VIDEO_RECORD)
		{
			video_encode_sensor_on(STATE_VIDEO_RECORD);
			//ap_video_record_resolution_display();
		}
		else
		{
			video_encode_sensor_on(STATE_PHOTO_CAPTURE);
			//ap_capture_resolution_display();
		}
		video_encode_preview_on(preview_mode);
	}
}

void switch_res_setting_change_res(void)
{
	if((present_state==STATE_VIDEO_RECORD)||(present_state==STATE_PHOTO_CAPTURE))
	{
		ap_video_record_clear_resolution_str();
		ap_capture_resolution_clear();
		video_encode_sensor_off();
		OSTimeDly(2);
		
		if(present_state==STATE_VIDEO_RECORD)
		{
			video_encode_sensor_on(STATE_VIDEO_RECORD);
			//ap_video_record_resolution_display();
		}
		else
		{
			video_encode_sensor_on(STATE_PHOTO_CAPTURE);
			//ap_capture_resolution_display();
		}
		video_encode_preview_on(preview_mode);
	}
}
extern INT8U usb_no_sensor_show_timer_start;
static void uvc_host_enum_device(void)
{
       INT32U  MaxPacket;
	   
	#if defined(C_UVC_MSG_DEBUG) && C_UVC_MSG_DEBUG
	DBG_PRINT("Host: do uvc_host_enum_device\r\n");
	#endif

       if(gUSB20_11_Connect == 1)     // USB20
       	{
       	    if(drv_l2_usbh_ehci_enumeration()==FALSE)
       	    {
       	    usb_no_sensor_show_timer_start = 1;
			msgQSend(ApQ, MSG_APQ_USB_DISCONNECT_SHOW, NULL, 0, MSG_PRI_NORMAL);
			DBG_PRINT("Host: MSG_USB_HOST_UVC_INITIAL enum failed\r\n");
			uvc_device_state = UVC_DEV_STATE_DEVICE_NOT_FOUND;
			gDetect_USBHUVC_Plug = 0;     // not detect usbh plug
			uvc_host_uninit();
			uvc_q_reset();
			/*#if USB_CAM_YUYV
			jpeg_enc_q_reset();
			#endif*/
			uvc_host_vbus_control(0);
			OSTimeDly(1);
			uvc_host_vbus_control(1);
			OSTimeDly(1);
			gUSB20_11_Connect = 1;
			uvc_host_init();
			gDetect_USBHUVC_Plug = 1;     // start detect usbh plug
			gUSB_CAM_CONNECT = 0;
			//OSTimeDly(50);
			//OSQPost(USBHUVCTaskQ, (void *)MSG_USB_HOST_UVC_REINITIAL);
       	    }
	    else
	    {
	        uvc_device_state = UVC_DEV_STATE_DEVICE_READY;
		 DBG_PRINT("Host: Enum UVC device successfully reinit_flag[%d]cmd[%d]!\r\n", uvc_host_reinit_flag, uvc_host_current_cmd);
	        if(drv_l2_usbh_ehci_set_interface(&MaxPacket) == FALSE)
		{
			DBG_PRINT("Host: MSG_USB_HOST_UVC_INITIAL set interface failed\r\n");
			gDetect_USBHUVC_Plug = 0;     // not detect usbh plug
			uvc_host_uninit();
			uvc_q_reset();
			/*#if USB_CAM_YUYV
			jpeg_enc_q_reset();
			#endif*/
			uvc_host_vbus_control(0);
			OSTimeDly(1);
			uvc_host_vbus_control(1);
			OSTimeDly(1);
			gUSB20_11_Connect = 1;
			uvc_host_init();
			gDetect_USBHUVC_Plug = 1;     // start detect usbh plug
			gUSB_CAM_CONNECT = 0;
			//OSTimeDly(50);
			//    OSQPost(USBHUVCTaskQ, (void *)MSG_USB_HOST_UVC_REINITIAL);
		}
		else
		{
			uvc_init_jpeg_buffer();
			/* Enable ISO IN hardware function */
			drv_l2_usbh_ehci_start_uvc_hw_iso_in();
			OSQPost(USBHUVCTaskQ, (void *)MSG_USB_HOST_UVC_START_GET_DATA);
		}
	    }
       	}
	else      // USB11
	{
	    if(drv_l2_usbh_uvc_enumeration() == FALSE)
	    {
	    	usb_no_sensor_show_timer_start = 1;
			msgQSend(ApQ, MSG_APQ_USB_DISCONNECT_SHOW, NULL, 0, MSG_PRI_NORMAL);
	        DBG_PRINT("Host: MSG_USB_HOST_UVC_INITIAL enum failed\r\n");
       	        uvc_device_state = UVC_DEV_STATE_DEVICE_NOT_FOUND;
		 gDetect_USBHUVC_Plug = 0;     // not detect usbh plug
	        uvc_host_uninit();
		 uvc_q_reset();				
		 uvc_host_vbus_control(0);
		 OSTimeDly(1);
		 uvc_host_vbus_control(1);
		 OSTimeDly(1);
		 gUSB20_11_Connect = 0;
		 uvc_host_init();
		 gDetect_USBHUVC_Plug = 1;     // start detect usbh plug
		 gUSB_CAM_CONNECT = 0;
		// OSTimeDly(50);
		 //OSQPost(USBHUVCTaskQ, (void *)MSG_USB_HOST_UVC_REINITIAL);

		msgQSend(ApQ, MSG_APQ_USB_DISCONNECT_SHOW, NULL, 0, MSG_PRI_NORMAL);
	    }
	    else
	    {
	        uvc_device_state = UVC_DEV_STATE_DEVICE_READY;
		 DBG_PRINT("Host: Enum UVC device successfully reinit_flag[%d]cmd[%d]!!\r\n", uvc_host_reinit_flag, uvc_host_current_cmd);
	        uvc_init_jpeg_buffer();
               OSQPost(USBHUVCTaskQ, (void *) MSG_USB_HOST_UVC_START_GET_DATA);
	    }
	}
}

void uvc_host_restart(void)
{
	if(uvc_host_reinit_flag == UVC_NO_REINIT)
	{
		uvc_host_reinit_flag = UVC_USB_DEVICE_EXIST_REINIT;
		///OSQPost(USBHUVCTaskQ ,(void*)MSG_USB_HOST_UVC_PLUG_OUT);
		OSQPost(USBHUVCTaskQ ,(void*)MSG_USB_HOST_UVC_REINITIAL);
	}
}

INT32U uvc_host_reinit_flag_get(void)
{
	return uvc_host_reinit_flag;
}

INT32U uvc_host_get_device_status(void)
{
	return ((uvc_device_state == UVC_DEV_STATE_DEVICE_NOT_FOUND)?0:1);
}
#if C_PREVIEW_DISPLAY_ZOOM
extern INT8U ZOOM_STEP;
#endif
void state_usbh_uvc_entry(void *para)
{
	INT32U	msg_id;
	INT16U	QReceiveTimeout = 0;
	INT8U	err ,i;
	INT32S    nRet;
	UVC_FRAME_BUF_INFO	frame_buf_info;
	char* jpg_buf;
	INT32U jpg_len,j;
	static INT32U rp_cnt = 0;
	INT8U rp_sta = 0;

	INT8U* test_ptr;

	USBHUVCTaskQ = OSQCreate(usbh_uvc_task_q_stack, USBH_UVC_TASK_QUEUE_MAX);
	if(USBHUVCTaskQ == NULL)
	{
		DBG_PRINT("Host: [Warning] USBHUVCTaskQ is null! \r\n");
	}

	/* Create semaphore for waiting setup command done */
	uvc_sem = OSSemCreate(0);

	usb_phy_sem = OSSemCreate(1);

	DBG_PRINT("Host: Enter state_usbh_uvc_entry... \r\n");

#if (USBH_TYPE == USBH_OHCI_TYPE)
	/* Enable reverse parking detection */
	drv_l2_usbh_uvc_enable_get_reverse_parking_status(1, 250);
#endif
/*
#if 1
		drv_l2_usbh_uvc_set_frame_format(USBH_YUV_FORMAT);
		drv_l2_usbh_uvc_set_frame_width_height(640, 480);
#else
		drv_l2_usbh_uvc_set_frame_format(USBH_MJPEG_FORMAT);
		drv_l2_usbh_uvc_set_frame_width_height(1280, 720);
#endif
*/
	/* Enable host VBUS 5V */
	///uvc_host_vbus_control(1);

	OSTimeDly(3);

	while(1)
	{
		msg_id = (INT32U) OSQPend(USBHUVCTaskQ, QReceiveTimeout, &err);
		usb_phy_lock();
		//DBG_PRINT("msg_id = %x\r\n", msg_id);
		switch(msg_id & 0xFFF)
		{
			
		case MSG_USB_HOST_UVC_PLUG_IN:
			///#if defined(C_UVC_MSG_DEBUG) && C_UVC_MSG_DEBUG
			///	DBG_PRINT("Host: MSG_USB_HOST_UVC_PLUG_IN device state = %s\r\n", uvc_state_string[uvc_device_state]);
			///#endif
			DBG_PRINT("UVC PLUG IN!\r\n");
#if C_PREVIEW_DISPLAY_ZOOM
			ZOOM_STEP = 0;
#endif
           // Check_USB11_20_Connect();
			
			if(uvc_device_state == UVC_DEV_STATE_DEVICE_NOT_FOUND)
			{
				uvc_device_state = UVC_DEV_STATE_DEVICE_PLUG_IN;

				/* Init USB host */
				uvc_host_init();

				/* Enum UVC device */
				uvc_host_enum_device();
			}
			break;

		case MSG_USB_HOST_UVC_PLUG_OUT:
			
#if C_PREVIEW_DISPLAY_ZOOM
			ZOOM_STEP = 0;
#endif
			#if defined(C_UVC_MSG_DEBUG) && C_UVC_MSG_DEBUG
				DBG_PRINT("Host: MSG_USB_HOST_UVC_PLUG_OUT[%s] flag[%d] host io %d\r\n", uvc_state_string[uvc_device_state], uvc_host_reinit_flag, ap_peripheral_get_host_io_status());
			#endif
			uvc_q_reset();
			USB_first_jpg	= 0;
			if(uvc_device_state != UVC_DEV_STATE_DEVICE_NOT_FOUND)
			{
				usb_no_sensor_show_timer_start = 1;
				ap_usb_sensor_state_set(0);
			       DBG_PRINT("UVC PLUG OUT\r\n");
				if((uvc_device_state == UVC_DEV_STATE_GETTING_JPEG) &&(gUSB20_11_Connect == 1))
				{
				    gWait_Stop_Get_ISO_Data = 1;
			           for(i=0;i<0x10;i++)
			           {
			                if(gStop_Get_ISO_Data == 1)
			                {
			                    break;
			                }
			                else
			                {
			                    OSTimeDly(10);
			                }
			           }
				}
			}
				uvc_device_state = UVC_DEV_STATE_DEVICE_NOT_FOUND;
                             
			       ///gWait_Stop_Get_ISO_Data = 0;
				///gStop_Get_ISO_Data = 0;

				if(ap_video_record_sts_get() & VIDEO_RECORD_BUSY)
			       {
				    //stop recording
				    ap_video_record_func_key_active();
				    gContinue_Record = 1;
                             }
			       else
			       {
			           gContinue_Record = 0;
			       }
				/* un-init USB host */
				gDetect_USBHUVC_Plug = 0;     // not detect usbh plug
				uvc_host_uninit();

				uvc_q_reset();
			    /*#if USB_CAM_YUYV
			       jpeg_enc_q_reset();
	            #endif*/
				uvc_host_vbus_control(0);
		        OSTimeDly(1);
			    uvc_host_vbus_control(1);
				OSTimeDly(1);
				//gUSB20_11_Connect = 1;//yjw
				uvc_host_init();
				gDetect_USBHUVC_Plug = 1;    // start detect usbh plug
				gUSB_CAM_CONNECT = 0;
				
			//}			
			
			break;

		case MSG_USB_HOST_UVC_START_GET_DATA:
			uvc_host_reinit_flag = UVC_NO_REINIT;
			uvc_host_current_cmd = UVC_START_GET_FRAME_CMD;
			DBG_PRINT("Host: MSG_USB_HOST_UVC_START_GET_DATA[%s]\r\n", uvc_state_string[uvc_device_state]);
			if(uvc_device_state == UVC_DEV_STATE_DEVICE_READY)
			{
				ap_usb_sensor_state_set(1);
				uvc_device_state = UVC_DEV_STATE_GETTING_JPEG;
				
				if(gUSB20_11_Connect == 1)       //USB2.0
				{
					DBG_PRINT("USB2.0 Start to Get Data!\r\n");
					drv_l2_usbh_ehci_hw_iso_in_trans(1);								    
				}
				else      // USB1.1
				{
				DBG_PRINT("USB1.1 Start to Get Data!\r\n");
				    drv_l2_usbh_uvc_start_get_frame();
				    uvc_setup_cmd_lock();
				}

				DBG_PRINT("Host: MSG_USB_HOST_UVC_START_GET_DATA[%s]===\r\n", uvc_state_string[uvc_device_state]);
			}
			DBG_PRINT("Host: MSG_USB_HOST_UVC_START_GET_DATA[%s]end\r\n", uvc_state_string[uvc_device_state]);
			if(gContinue_Record == 1)
			{
			    gContinue_Record = 0;
			    ap_video_record_func_key_active();
			}
			OSTimeDly(USBH_UVC_TASK_QUEUE_WAIT_CNT);
			break;

		case MSG_USB_HOST_UVC_STOP_GET_DATA:
			uvc_host_current_cmd = UVC_STOP_GET_FRAME_CMD;
			DBG_PRINT("Host: MSG_USB_HOST_UVC_STOP_GET_DATA[%s]\r\n", uvc_state_string[uvc_device_state]);
			if(uvc_device_state == UVC_DEV_STATE_GETTING_JPEG)
			{
				uvc_device_state = UVC_DEV_STATE_DEVICE_READY;
#if (USBH_TYPE == USBH_OHCI_TYPE)
				drv_l2_usbh_uvc_stop_get_frame();
				uvc_setup_cmd_lock();
#endif
				DBG_PRINT("Host: MSG_USB_HOST_UVC_STOP_GET_DATA[%s]===\r\n", uvc_state_string[uvc_device_state]);
			}
			DBG_PRINT("Host: MSG_USB_HOST_UVC_STOP_GET_DATA[%s]end\r\n", uvc_state_string[uvc_device_state]);
			OSTimeDly(USBH_UVC_TASK_QUEUE_WAIT_CNT);
			break;

		case MSG_USB_HOST_UVC_ABUF_DONE:
		case MSG_USB_HOST_UVC_A_OVERFLOW_EVENT:
			++fps_uvc_frame;

			jpg_buf = (char*)drv_l2_usbh_ehci_get_uvc_frame_start_addr(0);
			jpg_len = drv_l1_usbh_ehci_get_iso_hw_addr(0) - (INT32U)jpg_buf;

			cache_invalid_range((INT32U)jpg_buf, jpg_len);

			/* Update frame a buffer */
			if(uvc_frame_drop_cnt)
			{
				--uvc_frame_drop_cnt;
				frame_buf_info.abuf_addr = DUMMY_BUFFER_ADDRS;
			}
			else
			{
				if(uvc_q_get_idle(&(frame_buf_info.abuf_addr)) == STATUS_FAIL)
				{
					frame_buf_info.abuf_addr = DUMMY_BUFFER_ADDRS;
				}
			}

			if(change_resolution_flag)
			{
				frame_buf_info.abuf_addr = DUMMY_BUFFER_ADDRS;
			}
			
			frame_buf_info.abuf_size = uvc_jpeg_frame_buf_size;
			frame_buf_info.bbuf_addr = NULL;
			frame_buf_info.bbuf_size = 0;
			drv_l2_usbh_ehci_uvc_update_frame_buf(&frame_buf_info);
			
			////DBG_PRINT("A:0x%x\r\n",frame_buf_info.abuf_addr);

			if(msg_id == MSG_USB_HOST_UVC_A_OVERFLOW_EVENT)
			{
				++fps_uvc_overflow;
				DBG_PRINT("UVC A buf overflow 0x%X(%d)\r\n", jpg_buf, jpg_len);
				if((INT32U)jpg_buf != DUMMY_BUFFER_ADDRS)
				{
					uvc_q_put_idle((INT32U)jpg_buf);
				}
				break;
			}

			// Send to decode
			if((INT32U)jpg_buf != DUMMY_BUFFER_ADDRS)
			{
				++fps_uvc_update;
			    for(j=(jpg_len-1);j>(jpg_len-256);j--)
				{
					if((jpg_buf[j]==0xD9)&&(jpg_buf[j-1]==0xFF))
					{
						jpg_len = j+1;
						break;
					}
				}
				fps_uvc_size += jpg_len;

				/* Check JPEG header for MJPEG */
				if(drv_l2_usbh_uvc_get_frame_format() == USBH_MJPEG_FORMAT)
				{
					test_ptr = (INT8U*) jpg_buf;

					if(test_ptr[0] != 0xFF || test_ptr[1] != 0xD8 || test_ptr[jpg_len-2] != 0xFF || test_ptr[jpg_len-1] != 0xD9)
					{
						//DBG_PRINT("UVC A buf drop 0x%X(%d) =%X%X...%X%X\r\n", jpg_buf, jpg_len, test_ptr[0], test_ptr[1], test_ptr[jpg_len-2], test_ptr[jpg_len-1]);
						//while(1);
						uvc_q_put_idle((INT32U)jpg_buf);
						break;
					}
				}

				g_usb_host_UVC_data_error_count = 0;    // clear data error count
				///DBG_PRINT("A buf ready!!\r\n");
				if(drv_l2_usbh_uvc_get_frame_format() == USBH_MJPEG_FORMAT)
					uvc_q_put_ready((INT32U)jpg_buf, jpg_len);
				else if(drv_l2_usbh_uvc_get_frame_format() == USBH_YUV_FORMAT)
					uvc_q_put_idle((INT32U)jpg_buf);
				//DBG_PRINT("A...!!\r\n");
			}
			else
			{
				//DBG_PRINT("A dummy!!\r\n");
			}

			if(jpg_len > fps_uvc_max_size)
			{
				fps_uvc_max_size = jpg_len;
			}
			OSTimeDly(5);
			break;
			
		case MSG_USB_HOST_UVC_BBUF_DONE:
		case MSG_USB_HOST_UVC_B_OVERFLOW_EVENT:
			++fps_uvc_frame;

			jpg_buf = (char*)drv_l2_usbh_ehci_get_uvc_frame_start_addr(1);
			jpg_len = drv_l1_usbh_ehci_get_iso_hw_addr(1) - (INT32U)jpg_buf;

			cache_invalid_range((INT32U)jpg_buf, jpg_len);

			/* Update frame b buffer */
			if(uvc_frame_drop_cnt)
			{
				--uvc_frame_drop_cnt;
				frame_buf_info.bbuf_addr = DUMMY_BUFFER_ADDRS;
			}
			else
			{
				if(uvc_q_get_idle(&(frame_buf_info.bbuf_addr)) == STATUS_FAIL)
				{
					frame_buf_info.bbuf_addr = DUMMY_BUFFER_ADDRS;
				}
			}
			if(change_resolution_flag)
			{
				frame_buf_info.bbuf_addr = DUMMY_BUFFER_ADDRS;
			}
			frame_buf_info.abuf_addr = NULL;
			frame_buf_info.abuf_size = 0;
			frame_buf_info.bbuf_size = uvc_jpeg_frame_buf_size;
			drv_l2_usbh_ehci_uvc_update_frame_buf(&frame_buf_info);
			
			///DBG_PRINT("B:0x%x\r\n",frame_buf_info.bbuf_addr);
			
			if(msg_id == MSG_USB_HOST_UVC_B_OVERFLOW_EVENT)
			{
				++fps_uvc_overflow;
				DBG_PRINT("UVC B buf overflow 0x%X(%d)\r\n", jpg_buf, jpg_len);
				if((INT32U)jpg_buf != DUMMY_BUFFER_ADDRS)
				{
					uvc_q_put_idle((INT32U)jpg_buf);
				}
				break;
			}

			// Send to decode
			if((INT32U)jpg_buf != DUMMY_BUFFER_ADDRS)
			{
				++fps_uvc_update;
			    for(j=(jpg_len-1);j>(jpg_len-256);j--)
				{
					if((jpg_buf[j]==0xD9)&&(jpg_buf[j-1]==0xFF))
					{
						jpg_len = j+1;
						break;
					}
				}
				fps_uvc_size += jpg_len;

				/* Check JPEG header for MJPEG */
				if(drv_l2_usbh_uvc_get_frame_format() == USBH_MJPEG_FORMAT)
				{
					test_ptr = (INT8U*) jpg_buf;

					if(test_ptr[0] != 0xFF || test_ptr[1] != 0xD8 || test_ptr[jpg_len-2] != 0xFF || test_ptr[jpg_len-1] != 0xD9)
					{
						//DBG_PRINT("[ERROR] B buf 0x%X(%d) = 0x%X 0x%X ... 0x%X 0x%X\r\n", jpg_buf, jpg_len, test_ptr[0], test_ptr[1], test_ptr[jpg_len-2], test_ptr[jpg_len-1]);
						//while(1);
						uvc_q_put_idle((INT32U)jpg_buf);
						break;
					}
				}

				g_usb_host_UVC_data_error_count = 0;    // clear data error count
				///DBG_PRINT("B buf ready!!\r\n");
				if(drv_l2_usbh_uvc_get_frame_format() == USBH_MJPEG_FORMAT)
					uvc_q_put_ready((INT32U)jpg_buf, jpg_len);
				else if(drv_l2_usbh_uvc_get_frame_format() == USBH_YUV_FORMAT)
					uvc_q_put_idle((INT32U)jpg_buf);
				//DBG_PRINT("B...!!\r\n");
			}
			else
			{
				//DBG_PRINT("B dummy!!\r\n");
			}

			if(jpg_len > fps_uvc_max_size)
			{
				fps_uvc_max_size = jpg_len;
			}
			OSTimeDly(5);	
			break;
			
		//case EHCI_HOST_UPDATE_FRAME_LIST_EVENT:
		case MSG_USB_HOST_UVC_UPDATE_FRAME_LIST_EVENT:
			//DBG_PRINT("Update frame list event\r\n");
			///if((msg_id & 0x1000) == 0)
			    drv_l2_usbh_ehci_framelist_update(0);
			///else
			///    drv_l2_usbh_ehci_framelist_update(1);
			    
			break;

		case MSG_USB_HOST_UVC_CHANGE_RESOLUTION:
			DBG_PRINT("Start Change Resolution!\r\n");
			////change_resolution_flag = 1;
			rear_jpeg_in_addr = 0;
			uvc_keep_buf = 0;
			uvc_host_reinit_flag = UVC_USB_DEVICE_EXIST_REINIT;
			gWait_Stop_Get_ISO_Data = 1;
			for(i=0;i<0x10;i++)
			{
			    if(gStop_Get_ISO_Data == 1)
			    {
			        break;
			    }
			    else
			    {
			        OSTimeDly(10);
			    }
			}
			
			if(drv_l2_usbh_ehci_stop_get_iso_data() == 0)
			{
			    DBG_PRINT("UVC Stop Get Data error!\r\n");
			    break;
			}
			gWait_Stop_Get_ISO_Data = 0;
			gStop_Get_ISO_Data = 0;
			iTD_group_idx = 0;
			drv_l2_usbh_ehci_change_resolution();
			///rtp_jpeg_resolution_set(gApp_Select_Res);
			///switch_res_setting_change_res();
			uvc_device_state = UVC_DEV_STATE_DEVICE_READY;
			uvc_host_reinit_flag = UVC_NO_REINIT;
			change_resolution_flag = 0;
			break;

		case MSG_USB_HOST_UVC_REINITIAL:
			DBG_PRINT("USB CAM Reinitial\r\n");

            uvc_host_reinit_flag = UVC_USB_DEVICE_EXIST_REINIT;
			gWait_Stop_Get_ISO_Data = 1;
				uvc_q_reset();
			USB_first_jpg	= 0;
			for(i=0;i<0x10;i++)
			{
			    if(gStop_Get_ISO_Data == 1)
			    {
			        break;
			    }
			    else
			    {
			        OSTimeDly(10);
			    }
			}
			DBG_PRINT("Reinitial #1\r\n");
			///gWait_Stop_Get_ISO_Data = 0;
			///gStop_Get_ISO_Data = 0;

            if(ap_video_record_sts_get() & VIDEO_RECORD_BUSY)
			{
				//stop recording
				ap_video_record_func_key_active();
				gContinue_Record = 1;
            }
			else
			{
				gContinue_Record = 0;
			}

			uvc_device_state = UVC_DEV_STATE_DEVICE_NOT_FOUND;
			uvc_host_reinit_flag = UVC_NO_REINIT;
			gDetect_USBHUVC_Plug = 0;     // not detect usbh plug
			DBG_PRINT("Reinitial #2\r\n");
			uvc_host_uninit();
			//uvc_q_reset();
			DBG_PRINT("Reinitial #3\r\n");
			uvc_host_vbus_control(0);
		    OSTimeDly(1);
			uvc_host_vbus_control(1);
			DBG_PRINT("Reinitial #4\r\n");
			gJpeg_clk_close = 1;
			OSTimeDly(1);
			R_SYSTEM_CLK_EN0 &= ~(1 << 12);
            R_SYSTEM_CLK_EN1  &= ~(1 << 10);
			OSTimeDly(10);
			DBG_PRINT("Reinitial #5\r\n");
			R_SYSTEM_CLK_EN0 |= (1 << 12);
            R_SYSTEM_CLK_EN1  |= (1 << 10);
			OSTimeDly(10);
			gJpeg_clk_close = 0;
			DBG_PRINT("Reinitial #6\r\n");
			//gUSB20_11_Connect = 1;//yjw
			uvc_host_init();
			gDetect_USBHUVC_Plug = 1;     // start detect usbh plug
			gUSB_CAM_CONNECT = 0;
		    DBG_PRINT("Reinitial #7\r\n");
			
            /*if(uvc_device_state == UVC_DEV_STATE_DEVICE_NOT_FOUND)
			{
				uvc_device_state = UVC_DEV_STATE_DEVICE_PLUG_IN;				
				
				uvc_host_enum_device();
			}*/
			break;
			
		default:
			break;
		}
		usb_phy_unlock();
	}

	/* Task exit */
	OSSemDel(uvc_sem, OS_DEL_ALWAYS, &err);
	OSSemDel(usb_phy_sem, OS_DEL_ALWAYS, &err);
}

#endif // ENABLE_REAR_SENSOR_FROM_HOST_UVC
